#include "trackball.h"

namespace visualization
{

namespace
{

	/*
	* Project an x,y pair onto a sphere of radius r OR a hyperbolic sheet
	* if we are away from the center of the sphere.
	*/
	double tb_project_to_sphere(double r, double x, double y)
	{
		double d, t, z;

		d = sqrt(x*x + y*y);
		if (d < r/* * SQRT2D2*/)
		{    /* Inside sphere */
			z = sqrt(r*r - d*d);
		}
		else
		{   /* On hyperbola */
			t = r / SQRT2;
			z = t*t / d;
		}

		return z;
	}
}

/*
* OK, simulate a track-ball.  Project the points onto the virtual
* trackball, then figure out the axis of rotation, which is the cross
* product of P1 P2 and O P1 (O is the center of the ball, 0,0,0)
* Note:  This is a deformed trackball-- is a trackball in the center,
* but is deformed into a hyperbolic sheet of rotation away from the
* center.  This particular function was chosen after trying out
* several variations.
*
* It is assumed that the arguments to this routine are in the range
* (-1.0 ... 1.0)
*/
Quaternion Trackball(double p1x, double p1y, double p2x, double p2y)
{
	if (p1x == p2x && p1y == p2y) {
		return Quaternion();
	}

	/*
	* First, figure out z-coordinates for projection of P1 and P2 to
	* deformed sphere
	*/
	Vector v1(p1x, p1y, tb_project_to_sphere(TRACKBALLSIZE,p1x,p1y));
	Vector v2(p2x, p2y, tb_project_to_sphere(TRACKBALLSIZE,p2x,p2y));
	Vector d = v2 - v1;
	Vector axis = Vector::Cross(v2, v1);

	double phi;  /* how much to rotate about axis */

	/*
	*  Figure out how much to rotate around that axis.
	*/

	double t = d.Norm() / (2.0 * TRACKBALLSIZE);

	/*
	* Avoid problems with out-of-control values...
	*/
	if (t > 1.0) t = 1.0;
	if (t < -1.0) t = -1.0;
	phi = 2 * asin(t);

	return AxisToQuat(axis,phi);
}

/*
*  Given an axis and angle, compute quaternion.
*/
Quaternion AxisToQuat(Vector &axis, double phi)
{
	axis.Normalize();
	return Quaternion(axis * sin(phi/2), cos(phi/2));
}

}

